#include "system.h"
#include "device.h"
#include "arm.h"
#include "ps2_remote_control.h"
#include "color.h"

#include "timer.h"
#include "usart.h"

void arm_remote_control()
{
//	if(!PS2_RedLight())
  {
    /* 夹手控制 */
    __HAL_TIM_SET_COMPARE(&htim4, folder_servo, 1100 + fabs(ps2_data.rx * 3));//增大，夹紧

    /* 拨杆控制 */
    __HAL_TIM_SET_COMPARE(&htim4, left_servo, 780 + fabs(ps2_data.ry * 2));    //增大，顺时针
    __HAL_TIM_SET_COMPARE(&htim4, right_servo, 1500 - fabs(ps2_data.ry * 2));  //增大，顺时针
//		
    /* 机械臂升降运动控制 */
    if (ps2_data.green == 1)
      __HAL_TIM_SET_COMPARE(&htim4, lift_servo, 100);//增大，夹紧

    else if (ps2_data.blue == 1)
      __HAL_TIM_SET_COMPARE(&htim4, lift_servo, 2500);//增大，夹紧

    if (ps2_data.pink == 1)    //按下粉色键，开始获取依次物块颜色
    {
      u8 col_val;
//			__HAL_TIM_SET_COMPARE(&htim4, folder_servo, 1600);
//			delay_ms(1000);
//			__HAL_TIM_SET_COMPARE(&htim4, folder_servo, 1400);
//			delay_ms(1000);
//      Get_average_RGB(10);
//      col_val = judge_color();
//      printf("%d\r\n", col_val);
      switch (col_val) {
        case 0: {
          printf("红色\r\n");
        }
          break;

        case 1: {
          printf("绿色\r\n");
        }
          break;

        case 2: {
          printf("蓝色\r\n");
        }
          break;

        case 3: {
          printf("白色\r\n");
        }
          break;

        case 4: {
          printf("黑色\r\n");
        }
          break;

        default:
          printf("未识别\r\n");
      }
    }
//		
////		if(PS2.PSB_KEY_RED==1)			//打印出RGB值，方便调试
////			printf("%d %d %d\r\n",col_inf.r_val,col_inf.g_val,col_inf.b_val);
  }
}
